'''
This demo show the communication interface of MR813 motion control board based on Lcm.
Dependency: 
- robot_control_cmd_lcmt.py
- robot_control_response_lcmt.py
'''
import lcm
import sys
import os
import time
from threading import Thread, Lock

from robot_control_cmd_lcmt import robot_control_cmd_lcmt
from robot_control_response_lcmt import robot_control_response_lcmt
     
def main():
    # 初始化机器人控制对象
    Ctrl = Robot_Ctrl()
    # 启动机器人控制
    Ctrl.run()
    
    # 创建控制命令消息
    msg = robot_control_cmd_lcmt()
    
    try:
        # 恢复站立
        msg.mode = 12  # Recovery stand
        msg.gait_id = 0
        msg.life_count += 1  # 命令将生效当 life_count 更新
        Ctrl.Send_cmd(msg)  # 发送命令
        Ctrl.Wait_finish(12, 0)  # 等待命令完成

        # 握手动作
        msg.mode = 62  # Shake hand, 基于位置插补控制
        msg.gait_id = 2
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        Ctrl.Wait_finish(62, 2)

        # 两腿站立
        msg.mode = 64  # Twoleg Stand
        msg.gait_id = 0
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        Ctrl.Wait_finish(64, 0)

        # 头部向上
        msg.mode = 21  # 位置插补控制
        msg.gait_id = 0
        msg.rpy_des = [0, 0.3, 0]  # 头部向上
        msg.duration = 500  # 预期执行时间，0.5秒
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(0.5)  # 等待执行

        # 头部向下
        msg.mode = 21  # 位置插补控制
        msg.gait_id = 0
        msg.rpy_des = [0, -0.3, 0]  # 头部向下
        msg.duration = 300  # 预期执行时间，0.3秒
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(0.3)  # 等待执行

        # 调整身体高度
        msg.mode = 21  # 位置插补控制
        msg.gait_id = 5
        msg.rpy_des = [0, 0, 0]
        msg.pos_des = [0, 0, 0.22]  # 设置身体高度
        msg.duration = 400  # 预期执行时间，0.4秒
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(1)  # 等待执行

        # 快速小跑
        msg.mode = 11  # Locomotion
        msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [0, 0, 0.5]  # 转向
        msg.duration = 0  # 零持续时间意味着持续运动，直到收到新命令
        msg.step_height = [0.06, 0.06]  # 摆动腿的地面间隙
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(5)  # 持续运动5秒

        # 纯阻尼模式
        msg.mode = 7  # PureDamper
        msg.gait_id = 0
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        Ctrl.Wait_finish(7, 0)

    except KeyboardInterrupt:
        # 捕获键盘中断，安全退出
        pass
    # 退出程序
    Ctrl.quit()
    sys.exit()

class Robot_Ctrl(object):
    def __init__(self):
        # 初始化接收和发送线程
        self.rec_thread = Thread(target=self.rec_responce)
        self.send_thread = Thread(target=self.send_publish)
        # 创建 LCM 客户端，用于接收
        self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255")
        # 创建 LCM 客户端，用于发送
        self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
        # 初始化命令消息
        self.cmd_msg = robot_control_cmd_lcmt()
        # 初始化响应消息
        self.rec_msg = robot_control_response_lcmt()
        # 创建锁，用于线程安全
        self.send_lock = Lock()
        # 初始化延迟计数
        self.delay_cnt = 0
        # 初始化模式和步态状态
        self.mode_ok = 0
        self.gait_ok = 0
        # 设置运行标志
        self.runing = 1

    def run(self):
        # 订阅机器人响应通道
        self.lc_r.subscribe("robot_control_response", self.msg_handler)
        # 启动发送线程
        self.send_thread.start()
        # 启动接收线程
        self.rec_thread.start()

    def msg_handler(self, channel, data):
        # 解析接收到的消息
        self.rec_msg = robot_control_response_lcmt().decode(data)
        # 检查命令执行进度
        if self.rec_msg.order_process_bar >= 95:
            # 更新当前模式
            self.mode_ok = self.rec_msg.mode
        else:
            # 重置模式状态
            self.mode_ok = 0

    def rec_responce(self):
        # 持续接收响应
        while self.runing:
            self.lc_r.handle()
            time.sleep(0.002)  # 短暂休眠，避免高CPU占用

    def Wait_finish(self, mode, gait_id):
        # 等待命令完成
        count = 0
        while self.runing and count < 2000:  # 最多等待10秒
            if self.mode_ok == mode and self.gait_ok == gait_id:
                # 命令完成，返回True
                return True
            else:
                # 等待并计数
                time.sleep(0.005)
                count += 1
        # 超时，返回False
        return False

    def send_publish(self):
        # 持续发送命令
        while self.runing:
            self.send_lock.acquire()
            if self.delay_cnt > 20:  # 心跳信号，10Hz
                # 发送命令
                self.lc_s.publish("robot_control_cmd", self.cmd_msg.encode())
                self.delay_cnt = 0  # 重置计数
            self.delay_cnt += 1
            self.send_lock.release()
            time.sleep(0.005)  # 短暂休眠

    def Send_cmd(self, msg):
        # 发送命令
        self.send_lock.acquire()
        self.delay_cnt = 50  # 确保命令立即发送
        self.cmd_msg = msg  # 更新命令消息
        self.send_lock.release()

    def quit(self):
        # 停止线程并清理资源
        self.runing = 0
        self.rec_thread.join()
        self.send_thread.join()

# 主函数
if __name__ == '__main__':
    main()